
#include <agv_peripheral_controller/ioLift/two_diff_io_lifter_controller.h>
#include <agv_srvs/srv/output_interface.hpp>

namespace two_diff_agv_peripheral
{

    CIoLift::CIoLift()
    {
    }

    CIoLift::~CIoLift()
    {
    }

    bool CIoLift::init(ros::ServiceClient &client)
    {
        m_client = client;
        return true;
    }

    // 举升上升
    bool CIoLift::up()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }
        // 如果到达上限位，停止完成此动作
        if (upTarget())
        {
            finish = stop();
            DEBUG_WARN_OUT("CIoLift:up target:key=" << ctrl[0].key
                                                    << ",set stop finish=" << finish);
            return finish;
        }

        // 控制上升输出

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = true;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_OUT("up:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升下降
    bool CIoLift::down()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 如果到达下限位，停止完成此动作
        if (downTarget())
        {
            finish = stop();
            DEBUG_WARN_OUT("CIoLift:down target:key=" << ctrl[0].key
                                                      << ",set stop finish=" << finish);
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = true;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = outputIo(ctrl);
        DEBUG_OUT("down:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升停止
    bool CIoLift::stop()
    {
        bool finish = false;

        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = false;
        }
        finish = outputIo(ctrl);

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 货物装填
    bool CIoLift::loadStatus()
    {
        bool load = false;
        // 如果不处在下限位，则一直报有货状态
        bool unload = downTarget() && !upTarget();
        load = !unload;
        return load;
    }

    // 上升到位
    bool CIoLift::upTarget()
    {
        bool ret = inputIo(upLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("upTarget ok");
        }
        return ret;
    }

    // 下降到位
    bool CIoLift::downTarget()
    {
        bool ret = inputIo(downLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("downTarget ok");
        }
        return ret;
    }

    // 控制IO点输出 param1:索引 param2:触发
    bool CIoLift::outputIo(IO_GROUP outIoGroup)
    {
        // 初始化是否完成
        bool ret = false;

        // 客户端是否有效
        if (!m_client.isValid())
        {
            DEBUG_ERROR_OUT("CIoLift:outputinterface. client is not valid");
            return ret;
        }

        if (m_outIoRecord == outIoGroup)
        {
            // DEBUG__OUT("CIoLift:outIoGroup is the same as last time");
            return true;
        }

        agv_srvs::srv::OutputsNew srv;
        srv.request.array = outIoGroup;
        if (m_client.call(srv) && srv.response.state)
        {
            ret = true;
            m_outIoRecord = outIoGroup;
            DEBUG_OUT("CIoLift:control io sucess ;");
        }
        else
        {
            ret = false;
            DEBUG_ERROR_OUT("CIoLift: control io fail;");
        }

        return ret;
    }

    ///////////////////////////////CIoLifterController 实现///////////////////////////////////////////////
    // 默认构造函数
    CIoLifterController::CIoLifterController()
    {
        m_init = false;
    }

    // 析构函数
    CIoLifterController::~CIoLifterController()
    {
        m_subIo.shutdown();
    }

    // 是否初始化完成
    bool CIoLifterController::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("IoLifterController init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CIoLifterController::init()
    {

        // 订阅输入信号话题/readInput
        m_subIoNew = m_priNh.subscribe("/readInputNew", 5, &CIoLifterController::readInputNewCallBack, this);
        m_outClient = m_priNh.serviceClient<agv_srvs::srv::OutputsNew>("/outputNew");

        // 初始化io key-名字
        agv_msgs::msg::PairTypeInt io;
        io.value = 0;
        //前升降-限位开关
		io.key = UP_LIMIT_FRONT;
        m_frontLift.upLimit.Update(io);
		io.key = DOWN_LIMIT_FRONT;
        m_frontLift.downLimit.Update(io);
		//后升降-限位开关
		io.key = UP_LIMIT_REAR;
        m_rearLift.upLimit.Update(io);
		io.key = DOWN_LIMIT_REAR;
        m_rearLift.downLimit.Update(io);
		//前升降控制
        // 液压举升-如果数量为2 默认第一个升，第二个io为降
        io.key = CONTROL_IO_FRONT;
        m_frontLift.ctrl.push_back(io);
		//后升降控制
		io.key = CONTROL_IO_REAR;
        m_rearLift.ctrl.push_back(io);
		
		// 急停按钮输入
        io.key = EMG_BUTTON;
        m_emgBtn.Update(io);
		
		//初始IO控制输出客户端
        m_frontLift.init(m_outClient);
        m_rearLift.init(m_outClient);

        m_init = true;
        return true;
    }

    // 举升上升
    bool CIoLifterController::up()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_frontLift.up();
        ret &= m_rearLift.up();
        DEBUG_WARN_OUT("IoLifterController:up :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 举升下降
    bool CIoLifterController::down()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        // 控制下降输出
        bool ret = true;
        ret &= m_frontLift.down();
        ret &= m_rearLift.down();
        DEBUG_WARN_OUT("IoLifterController:down :"
                       << ",ret=" << ret);

        finish = ret;

        return finish;
    }

    // 所有举升停止
    bool CIoLifterController::stop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_frontLift.stop();
        ret &= m_rearLift.stop();
        DEBUG_WARN_OUT("IoLifterController:stop :"
                       << ",ret=" << ret);
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 急停按下
    bool CIoLifterController::emgPressed()
    {
        bool ret = false;
        if (!initFinish())
        {
            return ret;
        }

        ret = inputIo(m_emgBtn);
        if (ret)
        {
            DEBUG_WARN_OUT("emg preesed");
        }

        return ret;
    }

    // 货物装填
    bool CIoLifterController::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }
        // 液压举升不在最低处，防止撞工位，使用高度作为有无货物的其中一种条件
        load = m_frontLift.loadStatus() || m_rearLift.loadStatus();
        return load;
    }

    // 新更新输入IO对象
    void CIoLifterController::updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj)
    {
        auto inputIo = inputIoObj.Get();
        std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
        for (auto &data : inputs_msg)
        {
            if (data.key == inputIo.key)
            {
                inputIoObj.Update(data);
                // DEBUG_OUT("updateInutIoNew:inputIo.key=" << inputIo.key << ",data.value=" << data.value);
                break;
            }
        }
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CIoLifterController::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
    {
        // 获取举升上下限位IO信号 并更新数据的时间戳
 
        updateInutIoNew(msg, m_frontLift.upLimit);
        updateInutIoNew(msg, m_frontLift.downLimit);
        updateInutIoNew(msg, m_rearLift.upLimit);
        updateInutIoNew(msg, m_rearLift.downLimit);
 

        // 获取急停的
        updateInutIoNew(msg, m_emgBtn);
    }

}; // end namespace
